package clubhouse.base;

import java.util.ListIterator;
import java.util.Vector;

/**
 * @author Anis Darwich & Lionel Croix - Simulation and experimentation on Robot in WSN - WnSN Lab - SJTU
 */
public class Landmarks {
	
	/**The set of currently known landmarks (after being processed).*/
	private Vector<double[]> landmarks;
	/**The set of real Landmarks (as generated).*/
	private Vector<double[]> realLandmarks;
	
	/**
	 * Constructs the Landmarks object, instantiate 2 Vectors:
	 * 		<br/>  - landmarks known landmarks which will be modified by error & anti-SLAM.
	 * 		<br/>  - realLandmarks known landmarks which will never be modified (simulate a node core).
	 */
	public Landmarks() {
		this.landmarks = new Vector<double[]>(10,1);
		this.realLandmarks = new Vector<double[]>(10,1);
	}
	
	/**
	 * Get the number of known landmarks.
	 * @return the size of the vector containing known landmarks.
	 */
	public int size() {
		return(this.landmarks.size());
	}
	
	/**
	 * Store the specified landmark in both vectors landmarks and realLandmarks.
	 * @param nodeid the id of the node to store.
	 * @param X the X coordinate of the node to store.
	 * @param Y the Y coordinate of the node to store.
	 */
	public void storeLandmark(int nodeid, double X, double Y) {
		this.realLandmarks.add(new double[] {nodeid, X, Y});
		this.landmarks.add(new double[] {nodeid, X, Y});
	}
	
	/**
	 * Remove the node with specified Id and store a new one with specified coordinate.
	 * @param nodeid the id of the node to replace.
	 * @param newX the new X coordinate to store.
	 * @param newY the new Y coordinate to store.
	 */
	public void replaceLandmark(int nodeid, double newX, double newY) {
		this.landmarks.remove(this.getLandmark(nodeid));
		this.landmarks.add(new double[] {nodeid, newX, newY});
	}
	
	/**
	 * Get the landmark with the specified id.
	 * @param nodeid the Id of the node to get.
	 * @return the double array representing the specified node [nodeId,X coordinate,Y coordinate], null if unknown node id
	 */
	public double[] getLandmark(int nodeid) {
		double[] temp = null, result = null;
        ListIterator<double[]> li = this.landmarks.listIterator();
        while(li.hasNext()){
        	temp = li.next();
        	if (temp[0] == nodeid) {
        		result = temp;
        		break;
        	}
        }
		return result;
	}
	
	/**
	 * Get all known landmarks.
	 * @return the whole vector containing landmarks.
	 */
	public Vector<double[]> getLandmarks() {
		return this.landmarks;
	}
	
	/**
	 * Test if the specified landmark is known. 
	 * The exactly same landmark (nodeId,X coordinate,Y coordinate) has to be in landmarks vector.
	 * @param nodeid the id of node to test.
	 * @param X the X coordinate of node to test.
	 * @param Y the Y coordinate of node to test.
	 * @return true if the specified landmark is known, false otherwise.
	 */
	public boolean knownLandmark(int nodeid, double X, double Y) {
		return (this.getLandmark(nodeid)!= null);
	}
	
	/**
	 * Flush landmarks and realLandmarks vectors.
	 */
	public void reInitializeLandmarks() {
		this.landmarks = new Vector<double[]>(10,10);
		this.realLandmarks = new Vector<double[]>(10,10);
	}

	/**
	 * Get all known real landmarks as adding at the beginning.
	 * @return the whole vector containing real landmarks (never modified).
	 */
	public Vector<double[]> getRealLandmarks() {
		return this.realLandmarks;
	}
}
